#include "../ABB_functions_test.h"

//--Initializes variables and modes
void CZone::init(void)
{
    mPause = false;
    mRecChange = true;
    mRecStop = false;
    mRec = false;
    mModeChange = true;
    Move = false;
    Grip = false;
    Rec = false;

    mMode = 4;
    mPrevMode = mMode;
    mScale = 700;
    mZ = 240;

    con.writeRead((char *)"<Set TargetMode abs>", true);
    con.writeRead((char *)"<Set Interpol L>", true);
    std::cout << con.mBuffer << std::endl;

}

//--Converts float to string
std::string Convert (float number)
{
    std::ostringstream buff;
    buff<<number;

    return buff.str();
}

//--Sends a move command to the robot
void CZone::MoveRobot(void)
{
    std::string com;

    if((mK[1]+(mG[1]-mR[1])*mScale) < mZ)
        com = "<Move Target [[" + Convert(mK[2]+(mR[2]-mG[2])*mScale) + "," + Convert(mK[0]+(mR[0]-mG[0])*mScale) + ","+ Convert(mZ)+"],[0,0,0,0]]>";
    else
        com = "<Move Target [[" + Convert(mK[2]+(mR[2]-mG[2])*mScale) + "," + Convert(mK[0]+(mR[0]-mG[0])*mScale) + ","+ Convert(mK[1]+(mG[1]-mR[1])*mScale)+"],[0,0,0,0]]>";

    con.writeRead((char*)com.c_str(),false);
}

//--Sets the values for the different modes
void CZone::modeMenu(void)
{
    if(mPrevMode == 2)
    {
        mZ = 240.0;
        con.writeRead((char *)"<RunRoutine leavePen random>", true);
    }

    switch(mMode)
    {
    case 1://Abs mode, the normal mode for moving around and gripping stuff
    {
        Rec = true;
        Grip = true;
        Move = true;
    }
    break;
    case 2://Pen mode, retrieves the pen and allows you to paint without going through the table ...
    {
        Rec = true;
        Grip = false;
        Move = true;
        mZ = 245.0;
        con.writeRead((char *)"<RunRoutine RetrievPen random>", true);
    }
    break;
    case 3://Play, plays what has been recorded and then goes into limbo
    {
        Rec = false;
        Grip = false;
        Move = false;
        if(mRecStop) //If nothing has yet been recorded, nothing should be played.
            con.writeRead((char *)"<Play>",true);
        mMode = 4;
    }
    break;
    case 4://Limbo, a mode in which you could only change modes.
    {
        Rec = false;
        Grip = false;
        Move = false;
    }
    break;
    case 5://Quit
    {
        close(con.mSocket);
        ros::shutdown();
        Quit = true;
    }
    break;

    default:
        break;
    }
}

//--Gets the current position of the tool on the robot
void CZone::getK(void)
{
    char tmp[3][10];

    con.writeRead((char*)"<Get CurrentPose>", true); //Demand the current pose of the robot

    int c = 0;
    int pos = 0;

    //Take out relevant data from string
    for(int i = 0; con.mBuffer[i+3]!=']'; i++ )
    {
        if(con.mBuffer[i+3]==',')
        {
            c++;
            pos = i+1;
        }
        else
            tmp[c][i-pos]=con.mBuffer[i+3];
    }

    for(int i = 0; i<3; i++)
    {
        mK[(i+2)%3] = atof(tmp[i]);
        mG[i] = mR[i];
    }
}

bool CZone::isPauseZone(void)
{
    if(mL[1]<-0.25)
    {	
        mPause = true;

        if(mR[1]<-0.25 && mModeChange)
        {
            mModeChange = false;
            mMode = mMode%5;
            mMode++;
            std::cout << "Mode: " << mMode << std::endl;
        }
        else if(mR[1]>-0.1)
            mModeChange = true;
    }
    else if(mL[1]>-0.1)
    {
        if(mMode!=mPrevMode)
        {
            modeMenu();
            mPrevMode = mMode;
	    if(mMode==5)
		return false;
        }
        if(mPause)
            getK();
        mPause = false;
    }
}

void CZone::isRecZone(void)
{
    if(mL[3]-mL[2]>0.45 && mRecChange && !mRecStop)
    {
        mRecChange = false;
        if(!mRec)
        {
            con.writeRead((char *)"<Rec Start 1>",true);
            mRec = true;
        }
        else
        {
            con.writeRead((char *)"<Rec Stop mju.mod>",true);
            mRecStop = true;
        }
    }
    else if(mL[3]-mL[2]<0.25)
        mRecChange = true;


}
